/**
 * Edited by Adam Stroud on 12/19/2013
 * no changes made. Ball is localized, but motors do not move.
 *
 * Edited by Kellen Carey on 12/20/2013
 * Added console outputs to help with debugging
 * Source of error with motor motion arises from reading present location of the motors
 * 		When asked what the current location is, value is always zero.
 * 		We ran into this problem a long time ago, but don't recall what we changed to fix it.
 * 		So, for the short run, I set up variables that will hold the present positions, and every time
 * 			the position is changed, so is the variable. This is a very crude method, and we'll have to
 * 			figure out what's happening eventually.
 */

#include "opencv2/highgui/highgui.hpp"
#include "opencv2/imgproc/imgproc.hpp"
#include <cv.h>
#include <highgui.h>
#include <istream>
#include <stdio.h>
#include <math.h>

#include "Dyna.h"
#include "Vision.h"

using namespace cv;


//Motor Initialization
#define NUM_ACTUATOR 3
#define PI	3.141592f
int xmoto = 0;
int ymoto = 0;
Dyna vision_dyna;
int xmotor_position = CENTER_POSITION_23;
int ymotor_position = CENTER_POSITION_24;

// TODO For drawing image
IplImage* imgTracking;

// TODO FOr drawing image
void DrawLine(Mat img, Point start, Point end)
{
  int thickness = 2;
  int lineType = 8;
  line( img,
        start,
        end,
        CV_RGB(250,0,0),
        thickness,
        lineType );
}
// TODO FOr drawing image
void DrawGridFull(IplImage* img){
	  	//Draw grid on image
		/*
		 * Vertical lines
		 */
	  	//Left Side
	  	Point ptVFTLeft = Point(0,0);
	  	Point ptVFBLeft = Point(0,500);
	  	DrawLine(img,ptVFTLeft,ptVFBLeft);
	  	//Left-Middle
	  	Point ptVMTLeft = Point(213,0);
	  	Point ptVMBLeft = Point(213,500);
	  	DrawLine(img,ptVMTLeft,ptVMBLeft);
	  	//Right-Middle
	  	Point ptVMTRight = Point(426,0);
	  	Point ptVMBRight = Point(426,500);
	  	DrawLine(img,ptVMTRight,ptVMBRight);
	  	//Right Side
	  	Point ptVFTRight = Point(639,0);
	  	Point ptVFBRight = Point(639,500);
	  	DrawLine(img,ptVFTRight,ptVFBRight);
		/*
		 * Horizontal lines
		 */
	  	//Top Side
	  	Point ptHTLeft = Point(0,0);
	  	Point ptHTRight = Point(639,0);
	  	DrawLine(img,ptHTLeft,ptHTRight);
	  	//Top-Middle
	  	Point ptHMTLeft = Point(0,160);
	  	Point ptHMTRight = Point(639,160);
	  	DrawLine(img,ptHMTLeft,ptHMTRight);
	  	//Bottom-Middle
	  	Point ptHMBLeft = Point(0,320);
	  	Point ptHMBRight = Point(639,320);
	  	DrawLine(img,ptHMBLeft,ptHMBRight);
	  	//Bottom Side
	  	Point ptHBLeft = Point(0,479);
	  	Point ptHBRight = Point(639,479);
	  	DrawLine(img,ptHBLeft,ptHBRight);
}


//Find the position of the ball in the web cam grid (positions 1-9)
int findPos(IplImage* img, float xpos, float ypos){
	//Track left-side of View
	if(xpos <= 213 && ypos <=160){
		//left top
		return 1;
	}
	else if(xpos <= 213 && ypos >160 && ypos <=320){
		//left middle
		return 4;
	}
	else if(xpos <= 213 && ypos >320){
		//left bottom
		return 7;
	}
	//Track middle of view
	else if(xpos > 213 && xpos<=426 && ypos <=160){
		//center top
		return 2;
	}
	else if(xpos > 213 && xpos<=426 && ypos >160 && ypos <=320){
		//center middle
		return 5;
	}
	else if(xpos > 213 && xpos<=426 && ypos >320){
		//center bottom
		return 8;
	}
	//Track right-side of view
	else if(xpos>426 && ypos <=160){
		//right top
		return 3;
	}
	else if(xpos>426 && ypos >160 && ypos <=320){
		//right middle
		return 6;
	}
	else if(xpos>426 && ypos >320){
		//right bottom
		return 9;
	}
	return 0;
}

//Move the head motor incrementally to the grid position found in the findPos method
void moveHeadMotors(int gridPos){
			std::cout << xmotor_position << "\t" << ymotor_position << std::endl;
			int dMotor = 5;

			// TODO Optimize if it works

			// Right Bottom
			if(gridPos == 1){
				xmotor_position -= dMotor;
				ymotor_position -= dMotor;
				std::cout << "Right Bottom\t";
			}

			// Center Bottom
			else if(gridPos == 2){
				ymotor_position -= dMotor;
				std::cout << "Center Bottom\t";
			}

			// Left Bottom
			else if(gridPos == 3){
				xmotor_position += dMotor;
				ymotor_position -= dMotor;
				std::cout << "Left Bottom\t";
			}

			// Right Middle
			else if(gridPos == 4){
				xmotor_position -= dMotor;
				std::cout << "Right Middle\t";
			}

			// Center Middle
			else if(gridPos == 5){
				std::cout << "Center Middle\t";
			}

			// Left Middle
			else if(gridPos == 6){
				xmotor_position += dMotor;
				std::cout << "Left Middle\t";

			}

			// Right Top
			else if(gridPos == 7){
				xmotor_position -= dMotor;
				ymotor_position += dMotor;
				std::cout << "Right Top\t";
			}

			// Center Top
			else if(gridPos == 8){
				ymotor_position += dMotor;
				std::cout << "Center Top\t";
			}
			// Left Top
			else if(gridPos == 9){
				xmotor_position += dMotor;
				ymotor_position += dMotor;
				std::cout << "Left Top\t";
			}
			if (ymotor_position < 356)	{ ymotor_position = 356; }
			if (ymotor_position > 1916) { ymotor_position = 1916; }
			if (xmotor_position < 635)	{ xmotor_position = 635;  }
			if (xmotor_position > 3360)	{ xmotor_position = 3360; }

			vision_dyna.dxl_write_word(xmoto, GOAL_POSITION, xmotor_position);
			vision_dyna.dxl_write_word(ymoto, GOAL_POSITION, ymotor_position);
			return;
}

//move the body depending on where the head is located after it is in grid pos 5
int getBodyMovement(){
	int moveToLocation = -1;

	if(xmotor_position>=1885 && xmotor_position<=2115 && ymotor_position<500){
		//printf("\nKick");
		moveToLocation = KICK;
		std::cout << "Kicking" << std::endl;
	}
	else if(xmotor_position>=1885 && xmotor_position<=2115 && ymotor_position>=500 /*&& ymotoPos<=1030*/){
		//printf("\nMove Forward");
		moveToLocation = WALK;
		std::cout << "Walking" << std::endl;
	}
	else if(xmotor_position<1885 && ymotor_position>=500 /*&& ymotoPos<=1030*/){
		//printf("\nTurn Right");
		moveToLocation = TURNRIGHT;
		std::cout << "Turning right" << std::endl;
	}
	else if(xmotor_position>2115 && ymotor_position>=500 /*&& ymotoPos<=1030*/){
		//printf("\nTurn Left");
		moveToLocation = TURNLEFT;
		std::cout << "Turning left" << std::endl;
	}
	else if(xmotor_position<1885 && ymotor_position<500){
		//printf("\nStep Right");
		moveToLocation = STEPRIGHT;
		std::cout << "Stepping right" << std::endl;
	}
	else if(xmotor_position>2115 && ymotor_position<500){
		//printf("\nStep Left");
		moveToLocation = STEPLEFT;
		std::cout << "Stepping left" << std::endl;
	}

	return moveToLocation;
}

//Detect the largest red objects location in the webcam view
int detectContours(IplImage* img){
	int largest_area=90;
		int largest_contour_index=0;
		int moveBody = 0;
		Rect boundRect;

		IplImage* imgHSV = cvCreateImage(cvGetSize(img), IPL_DEPTH_8U, 3);
		cvCvtColor(img, imgHSV, CV_BGR2HSV); //Change the color format from BGR to HSV

		// Get Image Thresh
		//Set the thershold color (ball color) for image tracking
		IplImage* imgThresh = cvCreateImage(cvGetSize(imgHSV),IPL_DEPTH_8U, 1);
		cvInRangeS(imgHSV, cvScalar(0,137,150), cvScalar(26,256,241), imgThresh);

		cvSmooth(imgThresh, imgThresh, CV_GAUSSIAN,9,9);

		//CvSeq* result;   //hold sequence of points of a contour
		CvMemStorage *storage = cvCreateMemStorage(0); //storage area for all contours

		Mat imgThreshMat(imgThresh);
		Mat imgMat(img);
		vector<vector <Point> > contours; // Vector for storing contour
		vector<Vec4i> hierarchy;
		findContours(imgThreshMat, contours, hierarchy,CV_RETR_CCOMP, CV_CHAIN_APPROX_SIMPLE ); // Find the contours in the image

		for( int i = 0; i< contours.size(); i++ ) // iterate through each contour.
		{
			double a=contourArea( contours[i],false);  //Find the area of contour

			if(a>largest_area){
				largest_area=a; //Reset largest area
				largest_contour_index=i;  //Store the index of largest contour
				boundRect=boundingRect(contours[i]); // Find the bounding rectangle for biggest contour

				// For drawing image
				drawContours(imgMat, contours, i, Scalar(255,255,255), CV_FILLED, 8, hierarchy);
			}

		}

		Point centerRec = Point((boundRect.x + boundRect.width/2), (boundRect.y + boundRect.height/2));
		cvCircle(img, cvPoint(centerRec.x,centerRec.y), 4, CV_RGB(255,0,0), -1, 8, 0 );

		Scalar color(255,255,255);
		drawContours( imgThreshMat, contours,largest_contour_index, color, CV_FILLED, 1, hierarchy ); // Draw the largest contour using previously stored index.

		// TODO For showing image
		imshow("imgThreshMat", imgThreshMat);
		rectangle(imgMat, boundRect, Scalar(255,255,255), 3, 8, 0);

		//Find what grid the ball is in
		int gridPos = findPos(img, cvRound(centerRec.x),cvRound(centerRec.y));
		//if the ball is not in grid 5 (center) move head to grid 5
		if(gridPos!=0){moveHeadMotors(gridPos);}

	//	cvClearMemStorage(storage);

		cvReleaseMemStorage(&storage);
		//cvReleaseImage(&img);
		cvReleaseImage(&imgThresh);
		cvReleaseImage(&imgHSV);


		//return the grid position the ball is in to main.
		return gridPos;
}

//initialize the Dynamixel USB
void initMotorUSB(){

	int baudnum = 1;
	int deviceIndex = 0;
	///////// Open USB2Dynamixel ////////////
	if(vision_dyna.dxl_initialize(deviceIndex, baudnum) == 0 )
	{
		printf( "Failed to open USB2Dynamixel!\n" );
		printf( "Press Enter key to terminate...\n" );
		getchar();
	}
	else
		printf("Succeed to open USB2Dynamixel!\n");

	return;
}

//Set the motor numbers
int setMotor(char motorAxis){
	int motor;
	std::cout<< "Select " << motorAxis << "-motor"<< std::endl;
	std::cin >> motor;
	return motor;
}

//Set the speed of the motors
int setSpeed(){
	int speed;
	printf("Select motor speed (Between 1-70)");
	std::cin >> speed;
	return speed;
}

/** @function main */
int main(int argc, char** argv)
{
	initMotorUSB();
	xmoto = setMotor('X');
	ymoto = setMotor('Y');
	int motorSpeed = setSpeed();

	vision_dyna.dxl_write_word(ymoto, GOAL_MOVING_SPEED, motorSpeed);
	vision_dyna.dxl_write_word(xmoto, GOAL_MOVING_SPEED, motorSpeed);

	// TODO This doesn't get executed, why?
	vision_dyna.dxl_write_word(ymoto, GOAL_POSITION, ymotor_position);
	vision_dyna.dxl_write_word(xmoto, GOAL_POSITION, xmotor_position);

	CvCapture* capture =0;
	capture = cvCaptureFromCAM(0);
	if(!capture){
		printf("Capture failure\n");
		return -1;
	}

	IplImage* frame=0;
	frame = cvQueryFrame(capture);
	if(!frame)
	{ return -1; }

	int ballCurPos = 0;
	int bodyMovement = 0;
	int c;

	imgTracking=cvCreateImage(cvGetSize(frame), IPL_DEPTH_8U, 3);
	cvZero(imgTracking);

	cvNamedWindow("Video");

	//Iterate through the webcam frames
	while(true){
		ballCurPos = 0;
		bodyMovement = 0;
		frame = cvQueryFrame(capture);
		if(!frame) break;
		frame=cvCloneImage(frame);

		cvSmooth(frame, frame, CV_GAUSSIAN,3,3);

		//detectCircles(frame);
		ballCurPos = detectContours(frame);
		std::cout << ballCurPos << std::endl;
		// TODO For drawing image
		DrawGridFull(frame);
		cvShowImage("Video", frame);

//		if(ballCurPos == 5){
//			/*
//			 *  FINAL RETURN VALUE
//			 */
//			bodyMovement = getBodyMovement(); //retrieve body movement code (0-5)
//		}

		cvReleaseImage(&frame);


		c = cvWaitKey(5);
		if((char)c==27 ) break;
	}

	cvDestroyAllWindows();
	cvReleaseImage(&imgTracking);
	cvReleaseCapture(&capture);
	return 0;
}
